/*******************************************************************************
* 
* JTAG -  JTAG probe
* Copyright (C) 2009 Olivier Ringot <oringot@gmail.com>
* 
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
* 
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
* 
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, 
* Boston, MA  02110-1301, USA.
* 
********************************************************************************
* 
* $Revision: $
* $Date: $
* $Source: $
* $Log: $
*
********************************************************************************
*
* jtag_probe  
*          --device=<serial device>
*          --chip_erase
*
*******************************************************************************/

/*****************************************************************************
* headers
*****************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <getopt.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>

#include "picprog_serial.h"
#include "ihex.h"

/*****************************************************************************
* constants
*****************************************************************************/
#define NRST (1<<0)
#define TMS  (1<<1)
#define TDO  (1<<2)
#define TCK  (1<<3)
#define TDI  (1<<4)

#define MTAP_COMMAND        0x07
#define MTAP_SW_MTAP        0x04
#define MTAP_SW_ETAP        0x05
#define MTAP_IDCODE         0x01

#define MCHP_STATUS         0x00
#define MCHP_ASSERT_RST     0xd1
#define MCHP_DE_ASSERT_RST  0xd0
#define MCHP_ERASE          0xfc
#define MCHP_FLASH_ENABLE   0xfe
#define MCHP_FLASH_DISABLE  0xfd

#define ETAP_ADDRESS        0x08
#define ETAP_DATA           0x09
#define ETAP_CONTROL        0x0a
#define ETAP_EJTAGBOOT      0x0c
#define ETAP_FASTDATA       0x0e

/*****************************************************************************
* protypes
*****************************************************************************/
void setmode(unsigned int v,unsigned int k);
void sendcommand(unsigned int v);
unsigned int xferdata(unsigned int v);
unsigned int xferfastdata(unsigned int v);
unsigned int xferinstruction(unsigned int v);
void nrst_state(unsigned int v);
unsigned int peek(unsigned int addr);
void poke(unsigned int addr,unsigned int data);

/*****************************************************************************
* global variables
*****************************************************************************/
unsigned int device;

/*****************************************************************************
* main
*****************************************************************************/
int main(int argc,char **argv)
{
  /*****************************************************************************
  * variables declarations 
  *****************************************************************************/
  struct option longopts[]=
  {
     {"device",required_argument,0,0},
     {"chip_idcode",no_argument,0,0},
     {"chip_erase",no_argument,0,0},
     {"setmode",required_argument,0,0},
     {"sendcommand",required_argument,0,0},
     {"xferdata",required_argument,0,0},
     {"xferfastdata",required_argument,0,0},
     {"nrst_state",required_argument,0,0},
     {"dispcnt",no_argument,0,0},
     {"peek",required_argument,0,0},
     {"poke",required_argument,0,0},
     {"load_pe",required_argument,0,0},
     {"program",required_argument,0,0},
     {"enter_serial",no_argument,0,0},
     {"mfc0",required_argument,0,0},
     {"mtc0",required_argument,0,0},
     {"dump",required_argument,0,0},
     {"help",no_argument,0,0},
     {NULL,0,0,0}
  };
  
  int longindex,opt;
  char *device_name,*pe_name,*image_name;
  unsigned int  f_chip_idcode,f_chip_erase;
  unsigned int  f_setmode,setmode_val,setmode_len;
  unsigned int  f_sendcommand,sendcommand_val;
  unsigned int  f_xferdata,xferdata_val;
  unsigned int  f_xferfastdata,xferfastdata_val;
  unsigned int  f_nrst_state,nrst_state_val;
  unsigned int  f_help,f_dispcnt;
  unsigned int  f_peek,f_poke,f_addr,f_data,f_load_pe,f_program,f_enter_serial;
  unsigned int  f_mfc0,f_mtc0;
  unsigned int  f_dump;
  
  
  FILE *file_in;
  int i,v;
  unsigned int pracc;
  unsigned int status;
  unsigned int addr,data,last_page_erased,cp0_reg,cp0_sel,size;
  unsigned short crc;
  unsigned char buffer[128];
  
  struct s_ihex_node *node,*root_node;
  
  /*****************************************************************************
  * init
  *****************************************************************************/
  device_name  = "/dev/ttyUSB0";
  pe_name      = "./pe.hex";
  f_chip_idcode  = 0;
  f_chip_erase   = 0;
  f_setmode      = 0;
  f_sendcommand  = 0;
  f_xferdata     = 0;
  f_xferfastdata = 0;
  f_nrst_state   = 0;
  f_help         = 0;
  f_dispcnt      = 0;
  f_peek         = 0;
  f_poke         = 0;
  f_load_pe      = 0;
  f_program      = 0;
  f_enter_serial = 0;
  f_mfc0         = 0;
  f_mtc0         = 0;
  f_dump         = 0;
  
  /*****************************************************************************
  * parse command line argument
  *****************************************************************************/
  while(1)
  {
    opt = getopt_long_only(argc,argv,"",longopts,&longindex);
    
    if(opt==-1) break;
    if(opt!=0)
    {
      f_help=1;
      break;
    }
    
    switch(longindex)
    {
    
      case 0:
      
        device_name = strdup(optarg);
        break;
        
      case 1:
      
        f_chip_idcode = 1;
        break;

      case 2:
      
        f_chip_erase = 1;
        break;

      case 3:
      
        f_setmode   = 1;
        sscanf(optarg,"%x,%x",&setmode_val,&setmode_len);
        break;

      case 4:
      
        f_sendcommand = 1;
        sscanf(optarg,"%x,%x",&sendcommand_val);
        break;

      case 5:
      
        f_xferdata = 1;
        sscanf(optarg,"%x",&xferdata_val);
        break;

      case 6:
      
        f_xferfastdata = 1;
        sscanf(optarg,"%x",&xferfastdata_val);
        break;

      case 7:
      
        f_nrst_state = 1;
        sscanf(optarg,"%x",&nrst_state_val);
        break;

      case 8:
      
        f_dispcnt = 1;
        break;

      case 9:
      
        f_peek = 1;
        sscanf(optarg,"%x",&addr);
        break;
     
      case 10:
      
        f_poke = 1;
        sscanf(optarg,"%x,%x",&addr,&data);
        break;
 
      case 11:
      
        f_load_pe = 1;
        pe_name=strdup(optarg);
        break;

      case 12:
      
        f_program = 1;
        image_name=strdup(optarg);
        break;
  
      case 13:
      
        f_enter_serial = 1;
        break;

      case 14:
      
        f_mfc0=1;
        sscanf(optarg,"%x,%x",&cp0_reg,&cp0_sel);
        break;

      case 15:
      
        f_mtc0=1;
        sscanf(optarg,"%x,%x,%x",&cp0_reg,&cp0_sel,&data);
        break;

      case 16:
      
        f_dump=1;
        sscanf(optarg,"%x,%x",&addr,&size);
        break;

      default:
       
        f_help=1;
        break;
        
    }
  }
  
  /*****************************************************************************
  * help
  *****************************************************************************/
  if(f_help==1)
  {
    printf("JTAGPROBE V1.0 - Olivier Ringot (c) 2011\n\n");
    printf(" jtagprobe\n");  
    printf("     --device=<serial device>\n");  
    printf("     --chip_idcode\n");  
    printf("     --chip_erase\n");  
    printf("     --setmode=<val>,<len>\n");  
    printf("     --sendcommand=<val>\n");  
    printf("     --xferdata=<val>\n");  
    printf("     --xferfastdata=<val>\n");  
    printf("     --nrst_state=<val>\n");  
    printf("     --dispcnt=<val>\n");  
    printf("     --peek=<addr>\n");  
    printf("     --poke=<addr>,<data>\n");  
    printf("     --load_pe=<pe.hex>\n");  
    printf("     --program=<program.hex>\n");  
    printf("     --enter_serial\n");  
    printf("     --mfc0=<reg>,<sel>\n");  
    printf("     --mtc0=<reg>,<sel>,<value>\n");  
    printf("     --dump=<address>,<line>\n");  
    printf("     --help\n");  
    exit(-1); 
  }

  /*****************************************************************************
  * open serial device
  *****************************************************************************/
  if(device_name==0)
  {
    printf("error: device not specified !\n");
    exit(-1);
  }
  
  device=open(device_name,O_RDWR);
  if(device==-1)
  {
    printf("error: unable to open %s\n",device_name);
    exit(-1);
  }
  
  /*****************************************************************************
  * enter serial
  *****************************************************************************/
  if(f_enter_serial)
  {
    nrst_state(0);
    setmode(0x1f,6);
    sendcommand(0x05);
    sendcommand(0x0c);
    nrst_state(1);
  }

  /*****************************************************************************
  * query JTAG IDCODE
  *****************************************************************************/
  if(f_chip_idcode)
  {
    nrst_state(0x0);
    setmode(0x1f,6);
    sendcommand(0x1);
    status=xferdata(0);
    printf("idcode=0x%-.8x\n",status);
  }
 
  /*****************************************************************************
  * chip erase
  *****************************************************************************/
  if(f_chip_erase)
  {
    /* check device status */                             
    nrst_state(0x0);                                      
    printf(">checking device status : ");fflush(stdout);  
    setmode(0x1f,6);                                      
    sendcommand(MTAP_SW_MTAP);                            
    sendcommand(MTAP_COMMAND);                            
    while(1)                                              
    {                                                     
      status=xferdata(MCHP_STATUS);                       
      if(((status&0x08) && (~(status&0x4))))              
      {                                                   
        printf("OK\n");fflush(stdout);                    
        break;                                            
      }                                                   
      else                                                
      {                                                   
        printf(".");fflush(stdout);                       
        sleep(1);                                         
      }                                                   
    }                                                     
  
    /* erase device */                                    
    printf(">erasing device : ");fflush(stdout);          
    sendcommand(MTAP_SW_MTAP);                            
    sendcommand(MTAP_COMMAND);                            
    xferdata(MCHP_ERASE);                                 
    while(1)                                              
    {                                                     
      status=xferdata(MCHP_STATUS);                       
      if(((status&0x08) && (~(status&0x4))))              
      {                                                   
        printf("OK\n");fflush(stdout);                    
        break;                                            
      }                                                   
      else                                                
      {                                                   
        printf(".");fflush(stdout);                       
        sleep(1);                                         
      }                                                   
    } 
  }                                                   

  /*****************************************************************************
  * setmode
  *****************************************************************************/
  if(f_setmode)
  {
    setmode(setmode_val,setmode_len);
  }

  /*****************************************************************************
  * sendcommand
  *****************************************************************************/
  if(f_sendcommand)
  {
    sendcommand(sendcommand_val);
  }

  /*****************************************************************************
  * xferdata
  *****************************************************************************/
  if(f_xferdata)
  {
    status = xferdata(xferdata_val);
    printf("xferdata = 0x%-.8x\n",status);
    if(f_dispcnt)
    {
      printf("Rocc     = %d\n",(status>>31)&0x1);  
      printf("Psz      = %d",(status>>29)&0x3);
      
      switch((status>>29)&0x3)
      {
        case 0:  printf(" (byte)\n");break;
        case 1:  printf(" (half word)\n");break;
        case 2:  printf(" (word)\n");break;
        default: printf(" (triple)\n");break;
      }
        
      printf("Doze     = %d\n",(status>>22)&0x1);  
      printf("Halt     = %d\n",(status>>21)&0x1);  
      printf("PerRst   = %d\n",(status>>20)&0x1);  
      printf("PRnW     = %d\n",(status>>19)&0x1);  
      printf("PrAcc    = %d\n",(status>>18)&0x1);  
      printf("PrRst    = %d\n",(status>>16)&0x1);  
      printf("ProbEn   = %d\n",(status>>15)&0x1);  
      printf("ProbTrap = %d\n",(status>>14)&0x1);  
      printf("EjtagBrk = %d\n",(status>>12)&0x1);  
      printf("DM       = %d\n",(status>>3)&0x1);  
    }
  }

  /*****************************************************************************
  * xferfastdata
  *****************************************************************************/
  if(f_xferfastdata)
  {
    status = xferfastdata(xferfastdata_val);
    printf("xferfastdata = 0x%-.8x\n",status);
  
  }
 
  /*****************************************************************************
  * nrst_state
  *****************************************************************************/
  if(f_nrst_state)
  {
    nrst_state(nrst_state_val);
  }
  
  /*****************************************************************************
  * peek
  *****************************************************************************/
  if(f_peek)
  {
     status = peek(addr);
     printf("data = 0x%-.8x\n",status);
  }

  /*****************************************************************************
  * poke
  *****************************************************************************/
  if(f_poke)
  {
    poke(addr,data);
  }

  /*****************************************************************************
  * dump
  *****************************************************************************/
  if(f_dump)
  {
     unsigned int j;
     addr = addr&0xfffffffc;
     for(i=0;i<size;i++)
     {
       for(j=0;j<4;j++)
       {
         status        = peek(addr+i*16+j*4);
         buffer[j*4]   = status&0xff;
         buffer[j*4+1] = (status>>8)&0xff;
         buffer[j*4+2] = (status>>16)&0xff;
         buffer[j*4+3] = (status>>24)&0xff;
       }
     
       printf("%-.8x : ",addr + i*16);
       for(j=0;j<16;j++)
       {
         printf("%-.2x ",buffer[j]&0xff);
       }
      
       printf("  ");
       
       for(j=0;j<16;j++)
       {
         if((buffer[j]>=0x20) && (buffer[j]<0x7f))
         { 
           printf("%c",buffer[j]);
         }
         else
         {
           printf(".");
         }
       }
       printf("\n");
     }
  }


  
  /*****************************************************************************
  * pe
  *****************************************************************************/
  if(f_load_pe)
  {
    /* move to EJTAG BOOT */
    nrst_state(0);
    setmode(0x1f,6);
    sendcommand(MTAP_SW_ETAP);
    sendcommand(ETAP_EJTAGBOOT);
    nrst_state(1);
    
    /* init bus matrix */
    poke(0xbf882000,0x001f0040);
    poke(0xbf882010,0x00000800);
    poke(0xbf882020,0x00020000);
    poke(0xbf882030,0x00020000); 
    
    root_node = ihex_load(pe_name);
    
    /* new pe loader 
    
       <0x00000000> <base>        : base address of the 512 bytes to be loaded
       <0x00000001> <w0>.. <w127> : load 128 words (512 bytes) @address
       <0x00000002> <addr>        : jump at addr
    */
    
    printf("Uploading PE LOADER 0xa001f000\n");fflush(stdout);
    
    poke(0xa001f000,0x3c05ff20); //   0:   3c05ff20        lui     a1,0xff20
    poke(0xa001f004,0x8cba0000); //   4:   8cba0000        lw      k0,0(a1)
    poke(0xa001f008,0x13400007); //   8:   13400007        beqz    k0,0x28
    poke(0xa001f00c,0x00000000); //   c:   00000000        nop
    poke(0xa001f010,0x275affff); //  10:   275affff        addiu   k0,k0,-1
    poke(0xa001f014,0x13400007); //  14:   13400007        beqz    k0,0x34
    poke(0xa001f018,0x00000000); //  18:   00000000        nop
    poke(0xa001f01c,0x275affff); //  1c:   275affff        addiu   k0,k0,-1
    poke(0xa001f020,0x1340000b); //  20:   1340000b        beqz    k0,0x50
    poke(0xa001f024,0x00000000); //  24:   00000000        nop
    poke(0xa001f028,0x8ca40000); //  28:   8ca40000        lw      a0,0(a1)
    poke(0xa001f02c,0x1000fff5); //  2c:   1000fff5        b       0x4
    poke(0xa001f030,0x24860200); //  30:   24860200        addiu   a2,a0,512
    poke(0xa001f034,0x8cba0000); //  34:   8cba0000        lw      k0,0(a1)
    poke(0xa001f038,0xac9a0000); //  38:   ac9a0000        sw      k0,0(a0)
    poke(0xa001f03c,0x24840004); //  3c:   24840004        addiu   a0,a0,4
    poke(0xa001f040,0x1486fffc); //  40:   1486fffc        bne     a0,a2,0x34
    poke(0xa001f044,0x00000000); //  44:   00000000        nop
    poke(0xa001f048,0x1000ffee); //  48:   1000ffee        b       0x4
    poke(0xa001f04c,0x00000000); //  4c:   00000000        nop
    poke(0xa001f050,0x8cba0000); //  50:   8cba0000        lw      k0,0(a1)
    poke(0xa001f054,0x03400008); //  54:   03400008        jr      k0
    poke(0xa001f058,0x00000000); //  58:   00000000        nop
 
    xferinstruction(0x3c02a001);  /* lui    v0,0xa001              */
    xferinstruction(0x3442f000);  /* ori    v0,v0,0xf000           */
    xferinstruction(0x00400008);  /* jr     v0 ; execute PE LOADER */

    /* ensure that PE loader is executing  */
    while(1)
    {
      sendcommand(ETAP_CONTROL);
      status = xferdata(0x0004c000);
      if(!(status&0x00040000)) continue;
      sendcommand(ETAP_ADDRESS);
      status = xferdata(0x0);
      if(status == 0xff200000) break;
      xferinstruction(0x00000000); /* nop */
    }
    
    /* upload PE */

    sendcommand(MTAP_SW_ETAP);
    sendcommand(ETAP_FASTDATA);
    node = root_node;
    addr = node->address;
    
    while(node)
    {
      printf("Uploading PE 0x%-.8x\n",node->address);
      fflush(stdout);
      
      /* send starting address for the 512 byte */
      xferfastdata(0x00000000);
      xferfastdata(node->address);
       
      /* send 512 bytes */
      xferfastdata(0x00000001);
      for(i=0;i<512;i=i+4)
      {
        data = node->data[i];
        data |= node->data[i+1]<<8;
        data |= node->data[i+2]<<16;
        data |= node->data[i+3]<<24;
        xferfastdata(data);
      }
      node = node->next;
    }
 
    /* done */
    xferfastdata(0x00000002);
    xferfastdata(0xa0000800);
   
    ihex_free(root_node);
    if(f_program)
    {
      /* ensure that PE loader is executing  */
      while(1)
      {
        sendcommand(ETAP_CONTROL);
        status = xferdata(0x0004c000);
        if(!(status&0x00040000)) continue;
        sendcommand(ETAP_ADDRESS);
        status = xferdata(0x0);
        if(status == 0xff200000) break;
        xferinstruction(0x00000000);
      }
    }
    printf("\n");
  }
  
  if(f_program)
  {
    
    /* load image file */
    root_node = ihex_load(image_name);
    
    sendcommand(MTAP_SW_ETAP);
    sendcommand(ETAP_FASTDATA);
    
    /* upload pages */
    node = root_node;
    addr = -1;
    
    while(node)
    {
      printf("Uploading row 0x%-.8x\n",node->address);
      xferfastdata(0x10);
      xferfastdata(0xa0010000);
      xferfastdata(0x200);
      
      crc = 0;
      for(i=0;i<0x200;i=i+4)
      {
        data = node->data[i];
        data |= node->data[i+1]<<8;
        data |= node->data[i+2]<<16;
        data |= node->data[i+3]<<24;
        
        crc  = crc + (data)&0xffff;
        crc  = crc + (data>>8)&0xffff;
        crc  = crc + (data>>16)&0xffff;
        crc  = crc + (data>>24)&0xffff;
        
        xferfastdata(data);
      }
      
      status = xferfastdata(0x0);
      
      if(status!=crc)
      {
        printf("error:crc error on row upload (crc=%-.4x status=%-.4x) !\n",crc,status);
        exit(-1);
      }
      /* ack */
      xferfastdata(0x0);
      
      if(((node->address)&0x1ffff000)!=last_page_erased)
      {
        printf("Erasing page (0x%-.8x)\n",node->address);
        last_page_erased = (node->address)&0x1ffff000;
        xferfastdata(0x30);
        xferfastdata((node->address)&0x1fffffff);
        status = xferfastdata(0x0);
        if(status&0x3000)
        {
          printf("flash erasing error ! (NVMCON = 0x%-.8x)\n",status);
          exit(-1);
        }
        /* ack */
        xferfastdata(0x0);
      }
      
      printf("Programming row (0x%-.8x)\n",node->address);
      xferfastdata(0x40);
      xferfastdata(0xa0010000);
      xferfastdata((node->address)&0x1fffffff);
      status = xferfastdata(0x0);
      if(status&0x3000)
      {
        printf("flash programming error ! (NVMCON = 0x%-.8x)\n",status);
        exit(-1);
      }
      /* ack */
      xferfastdata(0x0);
      
      
      printf("Checking CRC row (0x%-.8x)\n",node->address);
      xferfastdata(0x50);
      xferfastdata((node->address)&0xffffffff);
      status = xferfastdata(0);
      if(status != crc)
      {
        printf("error:crc error on flashed row (crc=%-.4x status=%-.4x) !\n",crc,status);
        exit(-1);
      }      
      /* ack */
      xferfastdata(0x0);

      node = node->next;
    }
    
    ihex_free(root_node);
    printf("\n");
  }

  if(f_mfc0)
  {
    /*  3c04ff20 	lui    a0,0xff20
        40020000 	mfc0   v0,0,0
        ac820000 	sw     v0,0(a0)
    */
    xferinstruction(0x3c04ff20);
    xferinstruction(0x00000000);
    xferinstruction(0x00000000);
    xferinstruction(0x40020000 | ((cp0_reg&0x1f)<<11) | (cp0_sel&0x7));
    xferinstruction(0x00000000);
    xferinstruction(0x00000000);
    xferinstruction(0xac820000);
    xferinstruction(0x00000000);
    
    sendcommand(ETAP_DATA);                            
    status = xferdata(0);
    printf("mfc0 r%d,%d = 0x%-.8x\n",cp0_reg,cp0_sel,status);
  }

  if(f_mtc0)
  {
    /*  3c020000 	lui   v0,0x0
        34420000 	ori   v0,v0,0x0
        40820000 	mtc0  v0,0,0
    */
    xferinstruction(0x3c020000 | (data>>16)&0xffff);
    xferinstruction(0x34420000 | data&0xffff);
    xferinstruction(0x00000000);
    xferinstruction(0x00000000);
    xferinstruction(0x40820000 | ((cp0_reg&0x1f)<<11) | (cp0_sel&0x7));
    xferinstruction(0x00000000);
    xferinstruction(0x00000000);
  }
  
  /*****************************************************************************
  * close serial device
  *****************************************************************************/
  close(device);
}

/*********************************************************************************
* 
*********************************************************************************/
void setmode(unsigned int v,unsigned int k)
{
  send_char(device,0x0e);
  send_char(device,k);
  send_char(device,v);
  check_ack(device);
}

void sendcommand(unsigned int v)
{
  send_char(device,0x0f);
  send_char(device,v);
  check_ack(device);
}

unsigned int xferdata(unsigned int v)
{
  unsigned int r=0;
  send_char(device,0x10);
  send_char(device,v&0xff);
  send_char(device,(v>>8)&0xff);
  send_char(device,(v>>16)&0xff);
  send_char(device,(v>>24)&0xff);
 
  
  r=get_char(device)&0xff;
  r|=(get_char(device)&0xff)<<8;
  r|=(get_char(device)&0xff)<<16;
  r|=(get_char(device)&0xff)<<24;
  
  check_ack(device);
  return r;
}

unsigned int xferfastdata(unsigned int v)
{
  unsigned int r=0,timeout=0;
  unsigned char c;
  
  do
  {
    send_char(device,0x11);
    send_char(device,v&0xff);
    send_char(device,(v>>8)&0xff);
    send_char(device,(v>>16)&0xff);
    send_char(device,(v>>24)&0xff);
    r=get_char(device)&0xff;
    r|=(get_char(device)&0xff)<<8;
    r|=(get_char(device)&0xff)<<16;
    r|=(get_char(device)&0xff)<<24;
    c = get_char(device);
    
    if(c!=1)
    {
      timeout++;
    }
    
    if(timeout==1024)
    {
      printf("xferfastdata time-out !\n");
      exit(-1);
    }
    
  }
  while(c!=0x1); 
  
  return r;
}

void nrst_state(unsigned int v)
{
  send_char(device,0x12);
  send_char(device,v&0xff);
  check_ack(device);
}

unsigned int xferinstruction(unsigned int v)
{
  int status;
  sendcommand(MTAP_SW_ETAP);                               
  sendcommand(ETAP_CONTROL);
  do
  {
    status=xferdata(0x0004c000);
  }
  while(!(status&(1<<18)));
  sendcommand(ETAP_DATA);
  xferdata(v);
  sendcommand(ETAP_CONTROL);
  xferdata(0x0000c000);
}

unsigned int peek(unsigned int addr)
{
  unsigned int status;
 
  /* 3c04ff20   lui  a0,0xff20                        
   * 3c050000   lui  a1,addr[31:16]                   
   * 34a50000   ori  a1,a1,addr[15:0]                 
   * 8ca20000   lw   v0,0(a1)                         
   * ac820000   sw   v0,0(a0)                         
   */                                                 
  xferinstruction(0x3c04ff20);                       
  xferinstruction(0x3c050000 | (addr>>16)&0xffff);   
  xferinstruction(0x34a50000 | (addr)&0xffff    );   
  xferinstruction(0x8ca20000);                       
  xferinstruction(0xac820000);                       
                                                     
  /* wait new access pending */                      
  sendcommand(ETAP_CONTROL);                         
  do                                                 
  {                                                  
    status=xferdata(0x0004c000);                     
  }                                                  
  while(!(status&(1<<18)));                          
                                                   
  /* check address */                                
  while(1)                                           
  {                                                  
    sendcommand(ETAP_ADDRESS);                       
    status = xferdata(0x0);                          
                                                     
    if(status == 0xff200000)                         
    {                                                
      break;                                         
    }                                                
    else                                             
    {                                                
      xferinstruction(0x0); /* nop */                
    }                                                
  }                                                  
                                                     
  sendcommand(ETAP_DATA);                            
  status = xferdata(0x0);                            
                                                     
  /* finish access (sw) */                           
  sendcommand(ETAP_CONTROL);                         
  xferdata(0x0000c000); 
  return status;                             
}

void poke(unsigned int addr,unsigned int data)
{
    /*  3c050000  lui  a1,addr[31:16]
     *  34a50000  ori  a1,a1,addr[15:0]
     *  3c020000  lui  v0,data[31:16]
     *  34420000  ori  v0,v0,data[15:0]
     *  aca20000  sw   v0,0(a1)
     */
     xferinstruction(0x3c050000 | (addr>>16)&0xffff);
     xferinstruction(0x34a50000 | (addr)&0xffff);
     xferinstruction(0x3c020000 | (data>>16)&0xffff);
     xferinstruction(0x34420000 | (data)&0xffff);
     xferinstruction(0xaca20000);
     xferinstruction(0x0);
     xferinstruction(0x0);
}
